#pragma once

#include <gpiod.h>
#include <vector>
#include <string>
#include <chrono>
#include <thread>

#include <stdio.h>
#include <unistd.h>
#include <libserialport.h>

#include <iostream>
#include <iomanip>
#include <vector>
#include "rs485_v3.hpp"
#include "yj_return.hpp"
#include "config.h"


typedef enum
{
    LK_MOTOR_5015_CMD_READ_STATUS_1 = 0x9A,
    LK_MOTOR_5015_CMD_CLEAR_ERROR = 0x9B,
    LK_MOTOR_5015_CMD_CLOSE = 0x80,
    LK_MOTOR_5015_CMD_OPEN = 0x88,
    LK_MOTOR_5015_CMD_STOP = 0x81,
    LK_MOTOR_5015_CMD_MUTIL_CIRCLES_MODE_2 = 0xA4,
    LK_MOTOR_5015_CMD_SET_ENCODER_ZERO_TO_FLASH = 0x19,
    LK_MOTOR_5015_CMD_READ_MUTI_CIRCLES_ANGLE = 0x92,
    LK_MOTOR_5015_CMD_CLEAR_MUTI_CIRCLES_ANGLE = 0x93,
}lk_motor_5015_cmd_en;


typedef struct 
{
    uint8_t frame_head;
    uint8_t cmd;
    uint8_t motor_id;
    uint8_t data_frame_len;
    uint8_t cmd_checksum;
}lk_motor_5015_cmd_t,*Plk_motor_5015_cmd_t;

typedef struct 
{
    uint8_t temperature;
    float voltage;
    uint8_t status;
    uint8_t error;
}lk_motor_5015_status_1_t,*Plk_motor_5015_status_1_t;



class lk_motor_5015
{
    private:
    rs485_v3 *_Prs485;
    uint8_t _motor_id;

    yj_return init_flag;

    uint8_t _calculate_checksum(uint8_t *data, uint8_t len);//传入数据不包含checksum
    yj_return _check_checksum(uint8_t *data, uint8_t len);//传入数据包含checksum

    std::vector<uint8_t> _build_send_cmd_frame(lk_motor_5015_cmd_en cmd);//返回发送信息命令帧

    yj_return _send_cmd(lk_motor_5015_cmd_en cmd, void* data);
    yj_return _recv_msg(lk_motor_5015_cmd_en cmd, void* data);


    public:
    lk_motor_5015(rs485_v3 *Prs485,uint8_t motor_id);
    ~lk_motor_5015();

    yj_return lk_motor_5015_init(void);

    void lk_motor_5015_change_control_id(uint8_t new_id);//修改电机控制id 非电机id

    yj_return lk_motor_5015_muti_circle_angle_control_2(float angle, float max_speed);//多圈角度控制 speed单位为度每秒
    yj_return lk_motor_5015_read_status_1(Plk_motor_5015_status_1_t status_1);//读取状态1
    yj_return lk_motor_5015_clear_error();//清除错误
    
    yj_return lk_motor_5015_set_encoder_zero_to_flash();//当前位置设置编码器零点
    yj_return lk_motor_5015_read_muti_circle_angle(float *angle);//读取多圈角度
    yj_return lk_motor_5015_clear_muti_circle_angle();//清除多圈角度

    yj_return lk_motor_5015_close();
    yj_return lk_motor_5015_open();
    yj_return lk_motor_5015_stop();

    yj_return lk_motor_5015_move_to_muti_circle_angle_and_wait(float angle, float max_speed,float angle_tollerance = 0.15,uint64_t wait_time_ms = 3000);//多圈角度控制 speed单位为度每秒

};